﻿#include "utility.h"

void readConfig(std::string& serverIP, int& serverPort) {
    std::ifstream configFile("config.txt", std::ios::binary); // 二进制模式打开
    if (!configFile.is_open()) {
        std::cerr << "无法打开配置文件" << std::endl;
        exit(1);
    }

    // 检查并跳过UTF-8 BOM (EF BB BF)
    char bom[3] = { 0 };
    configFile.read(bom, 3);
    if (!(static_cast<unsigned char>(bom[0]) == 0xEF &&
        static_cast<unsigned char>(bom[1]) == 0xBB &&
        static_cast<unsigned char>(bom[2]) == 0xBF)) {
        // 如果不是BOM，回退文件指针
        configFile.seekg(0);
    }

    // 读取IP并清理行尾
    if (!std::getline(configFile, serverIP)) {
        std::cerr << "读取IP地址失败" << std::endl;
        exit(1);
    }

    // 去除行尾回车/换行符
    if (!serverIP.empty() && serverIP.back() == '\r') {
        serverIP.pop_back();
    }

    // 读取端口号
    if (!(configFile >> serverPort)) {
        std::cerr << "读取端口号失败" << std::endl;
        exit(1);
    }
}

void keyboardControl() 
{
    //std::lock_guard<std::mutex> lock(droneData.mtx);
    // 检测ESC键退出
    if (keystate(key_esc)) {
        std::cout << "正在退出程序..." << std::endl;
        droneData.Running = false;
        //Sleep(1000);
    }
    // 键盘控制pitch， yaw，roll
    if (keystate(key_W) && (droneData.pitch + 20) <= 2000) {
        droneData.pitch += 20;
    }
    if (keystate(key_A) && (droneData.yaw - 10) >= 1000) {
        droneData.yaw -= 10;
    }
    if (keystate(key_S) && (droneData.pitch - 20) >= 1000) {
        droneData.pitch -= 20;
    }
    if (keystate(key_D) && (droneData.yaw + 10) <= 2000) {
        droneData.yaw += 10;
    }
    if (keystate(key_E) && (droneData.roll - 10) >= 1000) {
        droneData.roll -= 10;
    }
    if (keystate(key_Q) && (droneData.roll + 10) <= 2000) {
        droneData.roll += 10;
    }
    // 下箭头键检测
    if (keystate(key_down) && (droneData.throttle - 3) >= 1000) {
        droneData.throttle -= 3;
    }
    // 上箭头键检测
    if (keystate(key_up) && (droneData.throttle + 3) <= 1350) {
        droneData.throttle += 3;
    }
}

void JoystickControl() {
    JOYINFOEX info; // 创建当前摇杆信息对象
    // 需要首先设定结构体中成员dwSize和dwFlags。
    info.dwSize = sizeof(JOYINFOEX); // 设置为JOYINFOEX结构体的大小
    info.dwFlags = JOY_RETURNBUTTONS | JOY_RETURNRAWDATA; // 获取按钮和原始数据

    // 检查摇杆是否连接
    if (joyGetNumDevs() > 0) {
        // 不断读取摇杆的当前状态，且返回
        int result = joyGetPosEx(JOYSTICKID1, &info); // 假设只有一个摇杆，使用JOYSTICKID1
        if (result == JOYERR_NOERROR) {
            droneData.throttle = info.dwZpos * (-1.9608) * 0.6 + 1300;
            //droneData.throttle = 2300 - droneData.throttle;
            droneData.yaw = info.dwRpos * 3.922 + 1000;
            droneData.roll = info.dwXpos / 5.461 / 3 + 1000;
            droneData.roll = 3000 - droneData.roll;
            droneData.pitch = info.dwYpos / 5.461 / 3 + 1000;
            droneData.pitch = 3000 - droneData.pitch;
        }
        else {
            //ege_drawtext("摇杆状态获取失败，错误代码", 0, 0);
        }
    }
    else {
        ege_drawtext("没有检测到摇杆设备", 0, 0);
    }
}